#include "features.h"

void showMatches(const Mat& pim1, vector<Point2f>& pts1, const Mat& pim2, vector<Point2f>& pts2, Mat& drawImg) {

	drawImg = Mat::zeros(pim1.rows, pim1.cols + pim2.cols, CV_8UC3);
	
	for (unsigned int iii = 0; iii < pim1.rows; iii++) {
		for (unsigned int jjj = 0; jjj < pim1.cols; jjj++) {
			
			drawImg.at<Vec3b>(iii,jjj)[0] = pim1.at<unsigned char>(iii,jjj);
			drawImg.at<Vec3b>(iii,jjj)[1] = pim1.at<unsigned char>(iii,jjj);
			drawImg.at<Vec3b>(iii,jjj)[2] = pim1.at<unsigned char>(iii,jjj);
			
			drawImg.at<Vec3b>(iii,640+jjj)[0] = pim2.at<unsigned char>(iii,jjj);
			drawImg.at<Vec3b>(iii,640+jjj)[1] = pim2.at<unsigned char>(iii,jjj);
			drawImg.at<Vec3b>(iii,640+jjj)[2] = pim2.at<unsigned char>(iii,jjj);
		}
	}
	
	Point pt1, pt2;
	int radius = 4;
	int thickness = 1;
	
	radius *= 16.0;
	for (unsigned int iii = 0; iii < pts1.size(); iii++) {
		
		pt1.x = 16.0 * pts1.at(iii).x;
		pt1.y = 16.0 * pts1.at(iii).y;
		
		pt2.x = 16.0 * (pts2.at(iii).x + 640.0);
		pt2.y = 16.0 * pts2.at(iii).y;
		
		circle(drawImg, pt1, radius, CV_RGB(255, 0, 0), thickness, CV_AA, 4);
		circle(drawImg, pt2, radius, CV_RGB(255, 0, 0), thickness, CV_AA, 4);
		line(drawImg, pt1, pt2, CV_RGB(0, 0, 255), thickness, CV_AA, 4);
	}
	
}

void crossCheckMatching( Ptr<DescriptorMatcher>& descriptorMatcher,
                         const Mat& descriptors1, const Mat& descriptors2,
                         vector<DMatch>& filteredMatches12, int knn )
{

    //printf("%s << ENTERED.\n", __FUNCTION__);

    filteredMatches12.clear();

    //printf("%s << DEBUG %d.\n", __FUNCTION__, 0);
    //vector<vector<DMatch> > matches12, matches21;
    vector<vector<DMatch> > matches12, matches21;


    //printf("%s << DEBUG %d.\n", __FUNCTION__, 1);

    //printf("%s << descriptors1.size() = (%d, %d).\n", __FUNCTION__, descriptors1.rows, descriptors1.cols);
    //printf("%s << descriptors2.size() = (%d, %d).\n", __FUNCTION__, descriptors2.rows, descriptors2.cols);



    descriptorMatcher->knnMatch( descriptors1, descriptors2, matches12, knn );


    //printf("%s << DEBUG %d.\n", __FUNCTION__, 2);
    descriptorMatcher->knnMatch( descriptors2, descriptors1, matches21, knn );

    //printf("%s << matches12.size() = %d.\n", __FUNCTION__, matches12.size());

    for( size_t m = 0; m < matches12.size(); m++ )
    {
        bool findCrossCheck = false;

        //printf("%s << matches12[%d].size() = %d.\n", __FUNCTION__, m, matches12[m].size());

        for( size_t fk = 0; fk < matches12[m].size(); fk++ )
        {
            DMatch forward = matches12[m][fk];

            //printf("%s << matches21[%d].size() = %d.\n", __FUNCTION__, forward.trainIdx, matches21[forward.trainIdx].size());

            for( size_t bk = 0; bk < matches21[forward.trainIdx].size(); bk++ )
            {
                DMatch backward = matches21[forward.trainIdx][bk];
                if( backward.trainIdx == forward.queryIdx )
                {
                    filteredMatches12.push_back(forward);
                    findCrossCheck = true;
                    break;
                }
            }
            if( findCrossCheck ) break;
        }
    }

}

void filterTrackingsByRadialProportion(vector<Point2f>& pts1, vector<Point2f>& pts2, Mat& K, Mat& newCamMat, Mat& distCoeffs, Size& imSize, double prop) {
	//cout << "filterTrackingsByRadialProportion: " << imSize.width << " " << imSize.height << endl;
	
	vector<Point2f> newPts;
	redistortPoints(pts2, newPts, K, distCoeffs, newCamMat);
	
	Point2f centrePt = Point2f(double(imSize.width)/2.0,double(imSize.height)/2.0);
	double maxDist = prop * double(imSize.width)/2.0;
	
	double dist;
	
	unsigned int iii = 0;
	
	while (iii < pts1.size()) {
		
		dist = distBetweenPts2f(pts2.at(iii), centrePt);
		
		if (dist > maxDist) {
			
			pts1.erase(pts1.begin() + iii);
			pts2.erase(pts2.begin() + iii);
			
		} else {
			iii++;
		}
		
	}

	
}

void reduceEdgyFeatures(vector<KeyPoint>& keypoints, cameraParameters& camData) {

	vector<Point2f> candidates;

	KeyPoint::convert(keypoints, candidates);

	vector<Point2f> redistortedPoints;
	float minBorderDist = 2.0;

	redistortPoints(candidates, redistortedPoints, camData.Kx, camData.distCoeffs, camData.expandedCamMat);

	for (int iii = candidates.size()-1; iii >= 0; iii--) {

		float xDist = min(abs(((float) camData.expandedSize.width) - redistortedPoints.at(iii).x), abs(redistortedPoints.at(iii).x));
		float yDist = min(abs(((float) camData.expandedSize.height) - redistortedPoints.at(iii).y), abs(redistortedPoints.at(iii).y));
		float dist = min(xDist, yDist) - keypoints.at(iii).size;

		if (dist < minBorderDist) {
			keypoints.erase(keypoints.begin() + iii);
		}

	}

}

void reduceEdgyCandidates(vector<Point2f>& candidates, cameraParameters& camData) {

	vector<Point2f> redistortedPoints;
	float minBorderDist = 2.0;

	redistortPoints(candidates, redistortedPoints, camData.Kx, camData.distCoeffs, camData.expandedCamMat);

	for (int iii = candidates.size()-1; iii >= 0; iii--) {

		float xDist = min(abs(((float) camData.expandedSize.width) - redistortedPoints.at(iii).x), abs(redistortedPoints.at(iii).x));
		float yDist = min(abs(((float) camData.expandedSize.height) - redistortedPoints.at(iii).y), abs(redistortedPoints.at(iii).y));
		float dist = min(xDist, yDist);
		if (dist < minBorderDist) {
			candidates.erase(candidates.begin() + iii);
		}

	}

}

bool checkRoomForFeature(vector<Point2f>& pts, Point2f& candidate, double dist) {

	double distance;

	for (unsigned int iii = 0; iii < pts.size(); iii++) {

		distance = distanceBetweenPoints(pts.at(iii), candidate);

		if (distance < dist) {
			return false;
		}

	}

	return true;

}

void reduceUnrefinedCandidates(vector<Point2f>& candidates) {

	if (candidates.size() == 0) {
		return;
	}

	for (int iii = candidates.size()-1; iii >= 0; iii--) {

		if (((candidates.at(iii).x - floor(candidates.at(iii).x)) == 0.0) && ((candidates.at(iii).y - floor(candidates.at(iii).y)) == 0.0)) {
			printf("%s << erasing: (%f, %f)\n", __FUNCTION__, candidates.at(iii).x, candidates.at(iii).y);
			candidates.erase(candidates.begin() + iii);
		}
	}

}

void reduceProximalCandidates(vector<Point2f>& existing, vector<Point2f>& candidates) {

	for (unsigned int iii = 0; iii < candidates.size(); iii++) {

		bool isValid = checkRoomForFeature(existing, candidates.at(iii), 3.0);

		if (!isValid) {
			candidates.erase(candidates.begin() + iii);
			iii--;
		}

	}

}

void concatenateWithExistingPoints(vector<Point2f>& pts, vector<Point2f>& kps, int size, double min_sep) {

	//printf("%s <<

	size = std::min(size, ((int) pts.size()) + ((int) kps.size()));

	// printf("%s << ready...\n", __FUNCTION__);

	for (unsigned int iii = 0; iii < kps.size(); iii++) {

		if (pts.size() >= size) {
			continue;
		}

		bool isValid = checkRoomForFeature(pts, kps.at(iii), min_sep);

		if (isValid) {
			pts.push_back(kps.at(iii));
		}

	}

}

void initializeDrawingColors(Scalar* kColors, Scalar* tColors, int num) {

	for (int iii = 0; iii < num; iii++) {
		switch (iii) {
			case 0:
				tColors[iii] = CV_RGB(255, 128, 128);
				kColors[iii] = CV_RGB(255, 0, 0);
				break;
			case 1:
				tColors[iii] = CV_RGB(128, 128, 255);
				kColors[iii] = CV_RGB(0, 0, 255);
				break;
			case 2:
				tColors[iii] = CV_RGB(128, 255, 128);
				kColors[iii] = CV_RGB(0, 255, 0);
				break;
			case 3:
				tColors[iii] = CV_RGB(128, 255, 255);
				kColors[iii] = CV_RGB(0, 255, 255);
				break;
			case 4:
				tColors[iii] = CV_RGB(128, 128, 255);
				kColors[iii] = CV_RGB(255, 0, 255);
				break;
			case 5:
				tColors[iii] = CV_RGB(255, 255, 128);
				kColors[iii] = CV_RGB(255, 255, 0);
				break;
			default:
				tColors[iii] = CV_RGB(128, 128, 255);
				kColors[iii] = CV_RGB(255, 255, 255);
				break;

		}
	}

}

void transformPoints(vector<Point2f>& pts1, Mat& H) {
	Mat ptCol(3, 1, CV_64FC1), newPos(3, 1, CV_64FC1);

	//vector<Point2f> tempPoints;
	//tempPoints.insert(tempPoints.end(), finishingPoints[kkk].begin(), finishingPoints[kkk].end());

	for (unsigned int mmm = 0; mmm < pts1.size(); mmm++) {

		ptCol.at<double>(0,0) = pts1.at(mmm).x;
		ptCol.at<double>(1,0) = pts1.at(mmm).y;
		ptCol.at<double>(2,0) = 1.0;

		newPos = H * ptCol;

		//printf("%s::%s << Changing point [%d][%d] from (%f, %f) to (%f, %f)\n", __PROGRAM__, __FUNCTION__, kkk, mmm, finishingPoints[kkk].at(mmm).x, finishingPoints[kkk].at(mmm).y, newPos.at<double>(0,0), newPos.at<double>(1,0));

		pts1.at(mmm).x = ((float) newPos.at<double>(0,0) / newPos.at<double>(2,0));
		pts1.at(mmm).y = ((float) newPos.at<double>(1,0) / newPos.at<double>(2,0));

	}
}

void reduceWeakFeatures(Mat& im, vector<KeyPoint>& feats, double minResponse) {

	// assignMinimumRadii(feats);

	vector<float> responseLevels;

	for (unsigned int iii = 0; iii < feats.size(); iii++) {

		if (feats.at(iii).response == 0.0) {
			feats.at(iii).response = estimateSalience(im, feats.at(iii).pt, ((double) feats.at(iii).size / 2.0));
			responseLevels.push_back(feats.at(iii).response);
		}

		//printf("%s << (%d) (%f, %f, %d)\n", __FUNCTION__, iii, feats.at(iii).size, feats.at(iii).response, feats.at(iii).octave);

	}

	sort(responseLevels.begin(), responseLevels.end());

	//printf("%s << minResponse = %f; maxResponse = %f; medianResponse = %f\n", __FUNCTION__, responseLevels.at(0), responseLevels.at(responseLevels.size()-1), responseLevels.at(responseLevels.size()/2));

	//cin.get();

	for (int iii = feats.size()-1; iii >= 0; iii--) {

		// responseLevels.at(responseLevels.size()-5)
		if (feats.at(iii).response < minResponse) { // 80.0
			feats.erase(feats.begin() + iii);
		}


	}

}

void trackPoints(Mat& im1, Mat& im2, vector<Point2f>& pts1, vector<Point2f>& pts2, int distanceConstraint, vector<unsigned int>& lostIndices, Mat H12, cameraParameters camData) {

	//struct timeval test_timer;
	//double testTime;
	
	bool debugFlag = true;
	
	if (debugFlag) {
		printf("%s << ENTERED!\n", __FUNCTION__);
	}
	
	//testTime = timeElapsedMS(test_timer, true);
	//printf("%s << ENTERED.\n", __FUNCTION__);

	if (pts1.size() == 0) {
		return;
	}

	if ((distanceConstraint % 2) == 0) {
		distanceConstraint++;
	}
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 0a: (%f)\n", testTime);

	Size winSize = Size(distanceConstraint, distanceConstraint);
	int maxLevel = 3;
	TermCriteria criteria = TermCriteria( TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01);
	double derivLambda = 0.5;
	int opticalFlowFlags;

	bool guiding = false;

	Mat im1b;
	vector<Point2f> pts1b;

	vector<uchar> statusVec;
	vector<float> err;

	Mat mask, blank;
	blank = Mat::ones(im1.size(), CV_8UC1);
	blank *= 255;
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 0b: (%f)\n", testTime);

	//printf("%s << DEBUG A\n", __FUNCTION__);

	/*
	double orig_vals[4], new_vals[4];
	findIntensityValues(orig_vals, im1, blank);
	// findIntensityValues(orig_vals, im1b, mask);
	findIntensityValues(new_vals, im2, blank);
	*/
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 0c: (%f)\n", testTime);

	//printf("%s << Vals (%f, %f, %f) vs (%f, %f, %f)\n", __FUNCTION__, orig_vals[0], orig_vals[1], orig_vals[2], new_vals[0], new_vals[1], new_vals[2]);

	vector<Point2f> originalPts;
	originalPts.insert(originalPts.end(), pts1.begin(), pts1.end());
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 1: (%f)\n", testTime);

	if ((H12.rows != 0) && (pts2.size() == pts1.size())) {

		// More aggressive shift for NUC handling
		/*
		double scale_shift = (new_vals[1] - new_vals[0]) / (orig_vals[1] - orig_vals[0]);
		double vert_shift = new_vals[0] - orig_vals[0];
		double down_shift = orig_vals[0];
		*/
		
		guiding = true;
		//printf("%s << Using initial guesses... (distanceConstraint = %d)\n", __FUNCTION__, distanceConstraint);
		opticalFlowFlags = OPTFLOW_USE_INITIAL_FLOW;

		pts1b.insert(pts1b.end(), pts1.begin(), pts1.end());
		transformPoints(pts1b, H12);
		warpPerspective(im1, im1b, H12, im1.size());

		//shiftIntensities(im1b, scale_shift, vert_shift, down_shift);

		warpPerspective(blank, mask, H12, blank.size());

		
	} else {

		/*
		double scale_shift = 1.0;
		double vert_shift = new_vals[3] - orig_vals[3];
		double down_shift = 0.0;
		*/
		
		im1b = Mat(im1); // .copyTo(im1b);
		// shiftIntensities(im1b, scale_shift, vert_shift, down_shift);

		// Need to make sure that black border stays as black, and only valid pixels are sampled...

		pts2.clear();
		opticalFlowFlags = 0;
	}

	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 2: (%f)\n", testTime);

	if (guiding) {
		calcOpticalFlowPyrLK(im1b, im2, pts1b, pts2, statusVec, err, winSize, maxLevel, criteria, derivLambda, opticalFlowFlags);
		//markStationaryPoints(pts1b, pts2, statusVec);
		
		
		markAbsentPoints(pts1b, pts2, statusVec, im1.size());

	} else {

		//printf("%s << Initial features: %d, %d\n", __FUNCTION__, pts1.size(), pts2.size());

		pts2.insert(pts2.end(), pts1.begin(), pts1.end());

		for (unsigned int iii = 0; iii < pts1.size(); iii++) {
			//printf("%s << pt(%d) = (%f, %f) vs (%f, %f)\n", __FUNCTION__, iii, pts1.at(iii).x, pts1.at(iii).y, pts2.at(iii).x, pts2.at(iii).y);
		}

		int opticalFlowFlags = OPTFLOW_USE_INITIAL_FLOW + OPTFLOW_LK_GET_MIN_EIGENVALS;
		double minEigThreshold = 1e-4;
		// calcOpticalFlowPyrLK(im1b, im2, pts1, pts2, statusVec, err, winSize, maxLevel, criteria, derivLambda, opticalFlowFlags, minEigThreshold); // opticalFlowFlags
		
		Mat im1b_x;
		im1b_x = Mat::zeros(im1b.size(), CV_8UC1);
		
		for (unsigned int iii = 0; iii < 475; iii++) {
			for (unsigned int jjj = 0; jjj < 635; jjj++) {
				im1b_x.at<unsigned char>(iii+5,jjj+5) = im1b.at<unsigned char>(iii,jjj);
			}
		}
		// im1b_x = im1b(Range(5, 479), Range(5, 639));
		
		calcOpticalFlowPyrLK(im1b, im1b_x, pts1, pts2, statusVec, err, winSize, maxLevel, criteria, derivLambda, opticalFlowFlags); // opticalFlowFlags
		//calcOpticalFlowPyrLK(im1b, im2, pts1, pts2, statusVec, err, winSize, maxLevel, criteria, derivLambda, opticalFlowFlags); // opticalFlowFlags

		

		if (debugFlag) {
			
			printf("%s << winSize = (%d, %d)\n", __FUNCTION__, winSize.width, winSize.height);
			
			Mat a, b;
			
			displayKeypoints(im1b, pts1, a, CV_RGB(255,0,0), 0);
			displayKeypoints(im1b_x, pts2, b, CV_RGB(255,0,0), 0);
			
			while (1) {
				
				imshow("test", a);
				waitKey(500);
				imshow("test", b);
				waitKey(500);
				
				printf("%s << After flow: %d / %d\n", __FUNCTION__, countNonZero(statusVec), statusVec.size());
			}
			
			for (unsigned int iii = 0; iii < pts1.size(); iii++) {
				// printf("%s << pts1.at(%d) = [%f, %f]; pts2.at(%d) = [%f, %f]\n", __FUNCTION__, iii, pts1.at(iii).x, pts1.at(iii).y, iii, pts2.at(iii).x, pts2.at(iii).y);
			}
			
			
		}
		
		for (unsigned int iii = 0; iii < pts1.size(); iii++) {
			//printf("%s << pt(%d) = (%f, %f) vs (%f, %f)\n", __FUNCTION__, iii, pts1.at(iii).x, pts1.at(iii).y, pts2.at(iii).x, pts2.at(iii).y);
		}
		
		//testTime = timeElapsedMS(test_timer, false);
		//printf("XDebug 2b: (%f)\n", testTime);

		//markStationaryPoints(pts1, pts2, statusVec);
		
		//printf("%s << After stationary: %d\n", __FUNCTION__, countNonZero(statusVec));
		
		markAbsentPoints(pts1, pts2, statusVec, im1.size());
		
		if (debugFlag) {
			printf("%s << After absent: %d\n", __FUNCTION__, countNonZero(statusVec));
		}
		

	}
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 3: (%f)\n", testTime);

	if (camData.Kx.rows != 0) {
		markEdgyTracks(pts2, statusVec, camData);

		//printf("%s << After edge filtering: %d\n", __FUNCTION__, countNonZero(statusVec));
	}
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 4: (%f)\n", testTime);

	//markBlandTracks(im2, pts2, statusVec, 1.0);

	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 5: (%f)\n", testTime);

	//printf("%s << After bland filtering: %d\n", __FUNCTION__, countNonZero(statusVec));

	//markUnrefinedPoints(pts2, statusVec);

	//printf("%s << After unrefined filtering: %d\n", __FUNCTION__, countNonZero(statusVec));

	if (guiding) {
		filterVectors(pts1, pts2, statusVec, ((double) std::max(im1.rows, im1.cols)), true);

		if (0) {

			Mat im1x, im2x;
			//warpPerspective(grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE], im2, H12, grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE].size());

			im1.copyTo(im1x);
			im2.copyTo(im2x);

			displayKeypoints(im1, pts1, im1x, CV_RGB(255,0,0), 0);
			displayKeypoints(im2x, pts2, im2x, CV_RGB(255,0,0), 0);

			//warpPerspective(im1, im1b, H12, im1.size());

			while (1) {

				imshow("temp_disp", im1x);	// Previous image with features
				waitKey(500);
				imshow("temp_disp", im2x);	// Current image
				waitKey(500);

			}
		}
	} else {
		//printf("%s << pts1.size() = %d [before filtering]\n", __FUNCTION__, pts1.size());
		//printf("%s << passed distanceConstraint = %d\n", __FUNCTION__, distanceConstraint);
		filterVectors(pts1, pts2, statusVec, ((double) distanceConstraint));
		//printf("%s << pts1.size() = %d [after filtering]\n", __FUNCTION__, pts1.size());

		//printf("%s << After full filtering: %d\n", __FUNCTION__, pts2.size());

		if (0) { // (pts2.size() < 5) {

			Mat im1x, im2x;
			//warpPerspective(grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE], im2, H12, grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE].size());

			cvtColor(im1b, im1x, CV_GRAY2RGB);
			cvtColor(im2, im2x, CV_GRAY2RGB);

			displayKeypoints(im1x, originalPts, im1x, CV_RGB(255,0,0), 0);
			displayKeypoints(im2x, pts2, im2x, CV_RGB(255,0,0), 0);

			//warpPerspective(im1, im1b, H12, im1.size());

			while (1) {

				imshow("temp_disp", im1b);	// Previous image with features
				waitKey(1500);
				imshow("temp_disp", im2x);	// Current image
				waitKey(500);

			}
		}

	}
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 6: (%f)\n", testTime);

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {
		if (statusVec.at(iii) == 0) {
			lostIndices.push_back(iii);
		}
	}
	
	//testTime = timeElapsedMS(test_timer, false);
	//printf("XDebug 7: (%f)\n", testTime);

}

void markAbsentPoints(vector<Point2f>&pts1, vector<Point2f>&pts2, vector<uchar>&statusVec, cv::Size imSize) {

	float buffer = 2.0;

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {

		if (statusVec.at(iii) != 0) {

			if ((pts1.at(iii).x < buffer) || (pts1.at(iii).x > imSize.width - buffer)) {
				statusVec.at(iii) = 0;
			} else if ((pts2.at(iii).x < buffer) || (pts2.at(iii).x > imSize.width - buffer)) {
				statusVec.at(iii) = 0;
			} else if ((pts1.at(iii).y < buffer) || (pts1.at(iii).y > imSize.height - buffer)) {
				statusVec.at(iii) = 0;
			} else if ((pts2.at(iii).y < buffer) || (pts2.at(iii).y > imSize.height - buffer)) {
				statusVec.at(iii) = 0;
			}

		}

	}

}

bool constructPatch(Mat& img, Mat& patch, Point2f& center, double radius, int cells) {

    //printf("%s << ENTERED.\n", __FUNCTION__);

	patch.release();

    if ((cells % 2) == 0) {
        cells++;
    }

    patch = Mat::zeros(cells, cells, CV_64FC1);

    int range = (cells-1)/2;

	for (int iii = -range; iii <= +range; iii++) {
		for (int jjj = -range; jjj <= +range; jjj++) {

			Point2f currPt = Point2f(center.x + ((double) iii)*radius, center.y + ((double) jjj)*radius);

			//printf("%s << currPt = (%f, %f)\n", __FUNCTION__, currPt.x, currPt.y);

			if ((currPt.x <= 0.0) || (currPt.x >= ((double) img.cols)) || (currPt.y >= ((double) img.rows)) || (currPt.y <= 0.0)) {
				return false;
			}

            //printf("%s << Extracting (%f, %f) of (%d, %d)\n", __FUNCTION__, currPt.x, currPt.y, img.cols, img.rows);
			double val = getInterpolatedVal(img, currPt);
			//printf("%s << Assigning (%d, %d) of (%d, %d)\n", __FUNCTION__, iii+range, jjj+range, cells, cells);
			patch.at<double>(iii+range,jjj+range) = val;

		}
	}

	//printf("%s << EXITING.\n", __FUNCTION__);

	return true;

}


void extendKeypoints(Mat& img, vector<KeyPoint>& pts, bool updateStrength, bool updateLocation) {

	if (updateLocation) {
		// Use cornersubpix to refine locations

		vector<Point2f> candidates;
		KeyPoint::convert(pts, candidates);

		cornerSubPix(img, candidates, Size(1,1), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));

		for (unsigned int iii = 0; iii < candidates.size(); iii++) {
			pts.at(iii).pt = candidates.at(iii);
		}

	}

	if (updateStrength) {



		for (unsigned int iii = 0; iii < pts.size(); iii++) {
			double radius = ((double) pts.at(iii).size) / 2.0;
			double salience = estimateSalience(img, pts.at(iii).pt, radius);
			pts.at(iii).response = salience;
		}

	}

}

double estimateSalience(Mat& img, Point2f& center, double radius) {

	// Assumes 8-bit image
	double salience = 0.0;



	// Find center
	//cv::Point center = Point(((int) kp.pt.x), ((int) kp.pt.y));

	// Find radius


	if (((center.x - radius) < 0) || ((center.y - radius) < 0) || ((center.x + radius) >= img.cols) || ((center.y + radius) >= img.rows)) {
		return salience;
	}

	Mat patch;
	int patchSize = radius * 2;
	if ((patchSize % 2) == 0) {
        //patchSize++;
	}

	constructPatch(img, patch, center, radius, patchSize);
	//cout << patch << endl;

	salience = getValueFromPatch(patch);


	return salience;

}

double getValueFromPatch(Mat& patch) {

    Mat convertedPatch = Mat::zeros(patch.size(), CV_32FC1);

	for (unsigned int iii = 0; iii < patch.rows; iii++) {
		for (unsigned int jjj = 0; jjj < patch.cols; jjj++) {
			convertedPatch.at<float>(iii,jjj) = ((float) patch.at<double>(iii,jjj));
		}
	}

    Mat eigenMat;
	cornerMinEigenVal(convertedPatch, eigenMat, patch.rows, CV_SCHARR);

    double eigenValue = 0.0, blankVal;

    minMaxLoc(eigenMat, &blankVal, &eigenValue);

    for (unsigned int iii = 0; iii < patch.rows; iii++) {
		for (unsigned int jjj = 0; jjj < patch.cols; jjj++) {
			//eigenValue += eigenMat.at<float>(iii,jjj) / ((float) (eigenMat.rows * eigenMat.cols));
		}
	}

	return eigenValue;

}

void markUnrefinedPoints(vector<Point2f>& pts, vector<uchar>&statusVec) {

	if (pts.size() == 0) {
		return;
	}

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {

		if (statusVec.at(iii) != 0) {

			if (((pts.at(iii).x - floor(pts.at(iii).x)) == 0.0) && ((pts.at(iii).y - floor(pts.at(iii).y)) == 0.0)) {
				statusVec.at(iii) = 0;
			}

		}
	}

}


void markBlandTracks(Mat& img, vector<Point2f>& pts, vector<uchar>& statusVec, double thresh) {

	if (pts.size() == 0) {
		return;
	}

	Mat patch;

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {

		if (statusVec.at(iii) != 0) {

			constructPatch(img, patch, pts.at(iii), 1.5);
			//cout << patch << endl;

			double salience = getValueFromPatch(patch);

			//printf("%s << pts.at(%d).response = %f (%f)\n", __FUNCTION__, iii, pts.at(iii).response, thresh);

			if (salience < thresh) {
				statusVec.at(iii) = 0;
			}


		}
	}


}

void filterBlandKeypoints(Mat& img, vector<KeyPoint>& pts, double thresh) {

	if (pts.size() == 0) {
		return;
	}

	for (int iii = pts.size()-1; iii >= 0; iii--) {

		//printf("%s << pts.at(%d).response = %f (%f)\n", __FUNCTION__, iii, pts.at(iii).response, thresh);

		if (pts.at(iii).response < thresh) {
			pts.erase(pts.begin() + iii);
		}

	}

}

void markStationaryPoints(vector<Point2f>&pts1, vector<Point2f>&pts2, vector<uchar>&statusVec) {

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {

		if (statusVec.at(iii) != 0) {

			double absDifference = pow(pow(pts1.at(iii).x - pts2.at(iii).x, 2.0) + pow(pts1.at(iii).y - pts2.at(iii).y, 2.0), 0.5);

			//printf("%s << absDifference = %f\n", __FUNCTION__, absDifference);

			if (absDifference == 0.0) {
				statusVec.at(iii) = 0;
			}

		}
	}

}

void displayKeypoints(const Mat& image, const vector<KeyPoint>& keypoints, Mat& outImg, const Scalar& color, int thickness) {

    bool randomColours = (color == Scalar::all(-1));

    Scalar newColour;

    if (!randomColours) {
        newColour = color;
    }

    image.copyTo(outImg);

    //printf("%s << DEBUG %d\n", __FUNCTION__, 0);

	/*
    if (image.channels() < 3) {
        cvtColor(image, outImg, CV_GRAY2RGB);
    } else {
        image.copyTo(outImg);
    }

    */

    //printf("%s << DEBUG %d\n", __FUNCTION__, 1);

    Point centerPt;

    int radius, crossLength; //, aimedSize;

    if (thickness == 0) {
        thickness = 1;
    }

    // New method:
	for (unsigned int i = 0; i < keypoints.size(); i++) {
		//int colorCoeff = (int)( (255.0 * ((double) keypoints.size() - (double)i)) / (double) keypoints.size());

		//printf("%s << %d: c = %d\n", __FUNCTION__, i, colorCoeff);
		//cin.get();
        newColour = color; // CV_RGB(255, 255-colorCoeff, 0);

        //printf("%s << DEBUG %d_%d\n", __FUNCTION__, 2, i);

        centerPt = keypoints.at(i).pt;

        centerPt.x *= 16.0;
        centerPt.y *=16.0;

        //aimedSize = 6 - ((int) 4.0*((double) i / (double) pts.size()));


        //radius = keypoints.at(i).size/2;

        //aimedSize = 6 - ((int) 4.0*((double) i / (double) keypoints.size()));
        radius = 16.0 * (keypoints.at(i).size/2.0); // 2
        //thickness = 3 - ((int) 2.0*((double) i / (double) keypoints.size()));
        //crossLength = int (1.5 * (double) aimedSize);
        crossLength = 2 * 16.0;

        if (radius > 0) {
            circle(outImg, centerPt, radius, newColour, thickness, CV_AA, 4);
        }

        line(outImg, Point(centerPt.x-crossLength, centerPt.y), Point(centerPt.x+crossLength, centerPt.y), newColour, thickness, CV_AA, 4);
        line(outImg, Point(centerPt.x, centerPt.y-crossLength), Point(centerPt.x, centerPt.y+crossLength), newColour, thickness, CV_AA, 4);

        //printf("%s << DEBUG %d_X_%d\n", __FUNCTION__, 2, i);

        //imshow("tempWin", outImg);

        //waitKey(200);

    }

    return;

}

void displayKeypoints(const Mat& image, const vector<Point2f>& pts, Mat& outImg, const Scalar& color, int thickness) {

    bool randomColours = (color == Scalar::all(-1));

    Scalar newColour;

    if (!randomColours) {
        newColour = color;
    }

    image.copyTo(outImg);

    Point centerPt;

    if (thickness == 0) {
        thickness = 1;
    }

    int crossLength; //, aimedSize;

    // New method:
	for (unsigned int i = 0; i < pts.size(); i++) {

        centerPt.x = pts.at(i).x * 16.0;
        centerPt.y = pts.at(i).y * 16.0;

        //aimedSize = 6 - ((int) 4.0*((double) i / (double) pts.size()));
        crossLength = 2 * 16.0;


        line(outImg, Point(centerPt.x-crossLength, centerPt.y), Point(centerPt.x+crossLength, centerPt.y), color, thickness, CV_AA, 4);
        line(outImg, Point(centerPt.x, centerPt.y-crossLength), Point(centerPt.x, centerPt.y+crossLength), color, thickness, CV_AA, 4);


    }

    return;

}

void sortKeypoints(vector<KeyPoint>& keypoints, unsigned int maxKeypoints) {
    vector<KeyPoint> sortedKeypoints;

    if (keypoints.size() <= 1) {
        return;
    }

    // Add the first one
    sortedKeypoints.push_back(keypoints.at(0));

    for (unsigned int i = 1; i < keypoints.size(); i++) {

        unsigned int j = 0;
        bool hasBeenAdded = false;

        while ((j < sortedKeypoints.size()) && (!hasBeenAdded)) {

            if (abs(keypoints.at(i).response) > abs(sortedKeypoints.at(j).response)) {
                sortedKeypoints.insert(sortedKeypoints.begin() + j, keypoints.at(i));

                hasBeenAdded = true;
            }

            j++;
        }

        if (!hasBeenAdded) {
            sortedKeypoints.push_back(keypoints.at(i));
        }

    }



    keypoints.swap(sortedKeypoints);

    if ((maxKeypoints != -1) && (maxKeypoints < ((int) keypoints.size()))) {
        keypoints.erase(keypoints.begin()+maxKeypoints, keypoints.end());
    }

    return;

}

void sortMatches(vector<vector<DMatch> >& matches1to2) {
	vector<vector<DMatch> > matchesCpy, newMatches;

	if (matches1to2.size() <= 1) {
		return;
	}

	matchesCpy.assign(matches1to2.begin(), matches1to2.end());

	while (matchesCpy.size() > 0) {
		double bestDistance = matchesCpy.at(0).at(0).distance;
		int bestIndex = 0;

		for (unsigned int iii = 0; iii < matchesCpy.size(); iii++) {

			if (matchesCpy.at(iii).at(0).distance <= bestDistance) {
				bestDistance = matchesCpy.at(iii).at(0).distance;
				bestIndex = iii;
			}
		}

		newMatches.push_back(matchesCpy.at(bestIndex));
		matchesCpy.erase(matchesCpy.begin() + bestIndex);
	}

	newMatches.swap(matches1to2);
}

void filterMatches(vector<vector<DMatch> >& matches1to2, double threshold) {

	if (matches1to2.size() == 0) {
		return;
	}

	int currIndex = matches1to2.size() - 1;

	while (matches1to2.at(currIndex).at(0).distance > threshold) {
		currIndex--;

		if (currIndex == -1) {
			matches1to2.clear();
			return;
		}
	}

	if (currIndex < ((int) matches1to2.size() - 1)) {
			matches1to2.erase(matches1to2.begin() + currIndex + 1, matches1to2.end());
	}


}

double distanceBetweenPoints(const Point2f& pt1, const Point2f& pt2) {
	double retVal;

	retVal = pow(pow(pt1.x - pt2.x, 2.0) + pow(pt1.y - pt2.y, 2.0), 0.5);

	return retVal;

}

double distanceBetweenPoints(const KeyPoint& pt1, const KeyPoint& pt2) {
	double retVal; //, a, b, c, d;

	/*
	a = pow(pt1.pt.x - pt2.pt.x, 2.0);
	b = pow(pt1.pt.y - pt2.pt.y, 2.0);
	c = pt1.pt.x - pt2.pt.x;
	d = pt1.pt.y - pt2.pt.y;
	*/

	//printf("%s << %f %f %f %f %f\n", __FUNCTION__, retVal, a, b, c, d);

	retVal = pow(pow(pt1.pt.x - pt2.pt.x, 2.0) + pow(pt1.pt.y - pt2.pt.y, 2.0), 0.5);

	return retVal;
}

void constrainMatchingMatrix(Mat& matchingMatrix, vector<KeyPoint>& kp1, vector<KeyPoint>& kp2, int distanceConstraint, double sizeConstraint) {

	int distanceBetween;

	for (unsigned int iii = 0; iii < kp1.size(); iii++) {
		for (unsigned int jjj = 0; jjj < kp2.size(); jjj++) {
			// Check distance constraint

			distanceBetween = (int) distanceBetweenPoints(kp1.at(iii), kp2.at(jjj));
			//printf("%s << distance (%d, %d) = %d (%f) vs %d\n", __FUNCTION__, iii, jjj, distanceBetween, distanceBetweenPoints(kp1.at(iii), kp2.at(jjj)), distanceConstraint);

			if (distanceBetween > distanceConstraint) {
				matchingMatrix.at<double>(iii,jjj) = -1;
			}

			//printf("%s << diameter = %f vs %f\n", __FUNCTION__, kp1.at(iii).size, kp2.at(jjj).size);

			if (max(kp1.at(iii).size/kp2.at(jjj).size, kp2.at(jjj).size/kp1.at(iii).size) > (1.00 + sizeConstraint)) {
				matchingMatrix.at<double>(iii,jjj) = -1;
			}

			//cin.get();
		}

		//cin.get();
	}


}

void twoWayPriorityMatching(Mat& matchingMatrix, vector<vector<DMatch> >& bestMatches) {

	bestMatches.clear();

    // void getPriorityScores(Mat& matchingMatrix, vector<vector<double> >& priorityScores, vector<vector<int> >& priorityMatches);

    // Potentially more efficient way of doing things:
        // Just search matrix for lowest match score, and record the next best in that 'cross' and blank out that cross,
        // then search for the lowest match score remaining in the matrix and so on and so forth...
        //
        // Actually many-many, 1-many, 1-1 can all use this basic approach, but just with different "blanking" strategies
        // e.g. 1-many would blank out the entire column, but not row
        // only many-many would not be able to record a valid "second best" score
            // and many-many would also have N^2 matches rather than just N for the others...

	bool rowsStillRemain = false;
	int remainingRows = 0;

	//printf("%s << Entered function.\n", __FUNCTION__);


	for (int iii = 0; iii < matchingMatrix.rows; iii++) {
		bool anyInThisRow = false;
		for (int jjj = 0; jjj < matchingMatrix.cols; jjj++) {
			if (matchingMatrix.at<double>(iii,jjj) >= 0) {
				rowsStillRemain = true;
				anyInThisRow = true;
			}
		}

		if (anyInThisRow) {
			remainingRows++;
		}

	}

	//printf("%s << remainingRows = %d\n", __FUNCTION__, remainingRows);

	while (rowsStillRemain) {

		double bestScore = 9e99;
		int bestRow = -1, bestCol = -1;

		for (int iii = 0; iii < matchingMatrix.rows; iii++) {
			for (int jjj = 0; jjj < matchingMatrix.cols; jjj++) {
				if ((matchingMatrix.at<double>(iii,jjj) <= bestScore) && (matchingMatrix.at<double>(iii,jjj) >= 0)) {
					bestScore = matchingMatrix.at<double>(iii,jjj);
					bestRow = iii;
					bestCol = jjj;
				}
			}
		}

		if ((bestScore < 0) || (bestScore == 9e99)) {
			rowsStillRemain = false;
			break;
		}

		matchingMatrix.at<double>(bestRow,bestCol) = -1.0;
		//matchingMatrixCpy.at<double>(bestRow,bestCol) = -1.0;

		DMatch currentMatch;
		vector<DMatch> currentMatchVector;

		currentMatch.queryIdx = bestRow;
		currentMatch.trainIdx = bestCol;

		double secondScore = 9e99;
		//int secondRow = -1, secondCol = -1;

		for (int iii = 0; iii < matchingMatrix.rows; iii++) {
			if ((matchingMatrix.at<double>(iii,bestCol) <= secondScore) && (matchingMatrix.at<double>(iii,bestCol) >= 0)) {
				secondScore = matchingMatrix.at<double>(iii,bestCol);
				//secondRow = iii;
				//secondCol = bestCol;
			}
			matchingMatrix.at<double>(iii,bestCol) = -1;
		}

		for (int iii = 0; iii < matchingMatrix.cols; iii++) {
			if ((matchingMatrix.at<double>(bestRow,iii) <= secondScore) && (matchingMatrix.at<double>(bestRow,iii) >= 0)) {
				secondScore = matchingMatrix.at<double>(bestRow,iii);
				//secondRow = bestRow;
				//secondCol = iii;
			}
			matchingMatrix.at<double>(bestRow,iii) = -1;
		}

		// If it is literally the last match available, it won't have a second best score...
		if (secondScore == 9e99) {
			secondScore = bestScore;
		}

		// Then normalize the "distance" based on this ratio, or can just use the ratio...
		//currentMatch.distance = bestScore;
		currentMatch.distance = bestScore / secondScore;
		//double gradient = -1.42;
		//double shift[3] = {1.86, 1.04, 0.21};

		//currentMatch.distance = reweightDistanceWithLinearSVM(bestScore, (bestScore/secondScore), gradient, shift);

		currentMatchVector.push_back(currentMatch);
		bestMatches.push_back(currentMatchVector);


	}

	//printf("%s << bestMatches.size() = %d\n", __FUNCTION__, bestMatches.size());
	//cin.get();

}

double calcDistance(double dist, double ratio, double *coeffs) {
    double retVal = 0.0;

    retVal = abs(coeffs[0] * dist - ratio + coeffs[2]) / sqrt(pow(coeffs[0], 2) + 1);

    return retVal;
}

double reweightDistanceWithLinearSVM(double dist, double ratio, double gradient) {
    double retVal = 0.0;    // ratio

    retVal = dist - (1.0 / gradient) * ratio;

    return retVal;
//
//    double LineNeg1[3], LineZero[3], LinePos1[3];
//
//    LineNeg1[0] = LineZero[0] = LinePos1[0] = gradient;
//    LineNeg1[1] = LineZero[1] = LinePos1[1] = -1;
//    LineNeg1[2] = shift[0];
//    LineZero[2] = shift[1];
//    LinePos1[2] = shift[2];
//
//    // Find distance between lines
//    //double negSeg = calcLinePerpDistance(LineNeg1, LineZero);
//    //double posSeg = calcLinePerpDistance(LinePos1, LineZero);
//
//    //printf("%s << Distances between lines = (%f, %f)\n", __FUNCTION__, negSeg, posSeg);
//
//    // Find distance from point to negative line
//    double negDist = calcDistance(dist, ratio, LineNeg1);
//
//    // Find distance from point to neutral line
//    double zeroDist = calcDistance(dist, ratio, LineZero);
//
//    // Find distance from point to positive line
//    double posDist = calcDistance(dist, ratio, LinePos1);
//
//    //printf("%s << distances = (%f, %f, %f)\n", __FUNCTION__, negDist, zeroDist, posDist);
//    //cin.get();
//
//    if (posDist < negDist) {
//        retVal = -1.0 * zeroDist;
//    } else {
//        retVal = +1.0 * zeroDist;
//    }
//
//    retVal = pow(2.0, retVal);
//
//    // Assign total score
//    //printf("%s << dist/ratio = (%f, %f), retVal = %f\n", __FUNCTION__, dist, ratio, retVal);
//    //cin.get();
//
//
//    return retVal;
}

void createMatchingMatrix(Mat& matchingMatrix, const Mat& desc1, const Mat& desc2) {

    vector<vector<DMatch> > matches1to2, matches2to1;

	Ptr<DescriptorMatcher> dmatcher;

	if (desc1.type() == CV_8UC1) {
		dmatcher = DescriptorMatcher::create("BruteForce-Hamming");
	} else if ((desc1.type() == CV_64FC1) || (desc1.type() == CV_32FC1)) {
		dmatcher = DescriptorMatcher::create("BruteForce");
	} else {
		printf("%s << ERROR! desc1.type() (%d) unrecognized.\n", __FUNCTION__, desc1.type());
	}


	//printf("%s << desc1.rows = %d; desc2.rows = %d\n", __FUNCTION__, desc1.rows, desc2.rows);

	dmatcher->knnMatch( desc1, desc2, matches1to2, desc1.rows );
	dmatcher->knnMatch( desc2, desc1, matches2to1, desc2.rows );

    /*
    Mat matMatDisp;
    matMatDisp = normForDisplay(matchingMatrix);
    imshow("testWin", matMatDisp);
    waitKey();
    */

    matchingMatrix = Mat::zeros(matches1to2.size(), matches2to1.size(), CV_64FC1);

    Mat countMat = Mat::zeros(matches1to2.size(), matches2to1.size(), CV_64FC1);

    // IM 1 to IM 2
    for (unsigned int iii = 0; iii < matches1to2.size(); iii++) {
        for (unsigned int jjj = 0; jjj < matches1to2[iii].size(); jjj++) {
            matchingMatrix.at<double>(iii, matches1to2.at(iii).at(jjj).trainIdx) += matches1to2.at(iii).at(jjj).distance;
            countMat.at<double>(iii, matches1to2.at(iii).at(jjj).trainIdx) += 1.0;
        }
    }

    // IM 2 to IM 1
    for (unsigned int iii = 0; iii < matches2to1.size(); iii++) {
        for (unsigned int jjj = 0; jjj < matches2to1[iii].size(); jjj++) {
            matchingMatrix.at<double>(matches2to1.at(iii).at(jjj).trainIdx, iii) += matches2to1.at(iii).at(jjj).distance;
            countMat.at<double>(matches2to1.at(iii).at(jjj).trainIdx, iii) += 1.0;
        }
    }

    /*
    Mat matMatDisp = normForDisplay(countMat);
    imshow("testWin", matMatDisp);
    waitKey();
    */

    matchingMatrix /= 2.0;

}

void filterTrackedPoints(vector<uchar>& statusVec, vector<float>& err, double maxError) {

	for (unsigned int iii = 0; iii < statusVec.size(); iii++) {
		if (err.at(iii) > maxError) {
			statusVec.at(iii) = 0;
		}
	}

}

void randomlyReducePoints(vector<Point2f>& pts, int maxFeatures) {

	srand (time(NULL) );

	if (((int) pts.size()) <= maxFeatures) {
		return;
	}

	int randIndex;

	while (((int) pts.size()) > maxFeatures) {
		randIndex = rand() % pts.size();
		pts.erase(pts.begin() + randIndex);
	}

}

void filterVectors(vector<Point2f>& pts1, vector<Point2f>& pts2, vector<uchar>& statusVec, double distanceConstraint, bool epipolarCheck) {

	//printf("%s << sizes = (%d, %d)\n", __FUNCTION__, pts1.size(), pts2.size());

	double totalDist, xDist, yDist;

	for (int iii = statusVec.size()-1; iii >= 0; iii--) {
		if (statusVec.at(iii) == 0) {
			pts1.erase(pts1.begin() + iii);
			pts2.erase(pts2.begin() + iii);
		} else {
			// distance check
			xDist = pts1.at(iii).x - pts2.at(iii).x;
			yDist = pts1.at(iii).y - pts2.at(iii).y;
			totalDist = pow((pow((pts1.at(iii).x - pts2.at(iii).x), 2.0) + pow((pts1.at(iii).y - pts2.at(iii).y), 2.0)), 0.5);

			//printf("%s << totalDist (%f) ([%f, %f]) vs distanceConstraint (%f)\n", __FUNCTION__, totalDist, xDist, yDist, distanceConstraint);

			if (totalDist > distanceConstraint) {



				pts1.erase(pts1.begin() + iii);
				pts2.erase(pts2.begin() + iii);
			}
		}
	}

	//printf("%s << sizes2 = (%d, %d)\n", __FUNCTION__, pts1.size(), pts2.size());

	// Then some kind of epipolar geometry check:

	if (epipolarCheck) {
		Mat F, matchesMask_F_matrix;
		F = findFundamentalMat(Mat(pts1), Mat(pts2), FM_RANSAC, 1.00, 0.99, matchesMask_F_matrix);

		for (int iii = matchesMask_F_matrix.rows-1; iii >= 0; iii--) {
			//printf("%s << matchesMask_F_matrix(%d,0) = %d\n", __FUNCTION__, iii, matchesMask_F_matrix.at<unsigned char>(iii,0));
			if (matchesMask_F_matrix.at<unsigned char>(iii,0) == 0) {
				pts1.erase(pts1.begin() + iii);
				pts2.erase(pts2.begin() + iii);
			}
		}

		//printf("%s << sizes3 = (%d, %d)\n", __FUNCTION__, pts1.size(), pts2.size());
	}


}

bool checkSufficientFeatureSpread(vector<Point2f>& pts, Size matSize, int minFeaturesInImage) {

	Mat countMat;
	int horizontalDivisions = 4, verticalDivisions = 3;
	countMat = Mat::zeros(3, 4, CV_16UC1);
	int minPerSegment = minFeaturesInImage / (8 * horizontalDivisions * verticalDivisions);

	//printf("%s << minPerSegment = %d\n", __FUNCTION__, minPerSegment);

	for (unsigned int iii = 0; iii < pts.size(); iii++) {
		int r, c;
		r = (horizontalDivisions * pts.at(iii).x) / matSize.width;
		c = (verticalDivisions * pts.at(iii).y) / matSize.height;
		countMat.at<unsigned short>(r, c) += 1;
	}

	int badSegments = 0;

	for (int iii = 0; iii < countMat.rows; iii++) {
		for (int jjj = 0; jjj < countMat.cols; jjj++) {
			if (countMat.at<unsigned short>(iii, jjj) < minPerSegment) {
				badSegments++;
			}
		}
	}

	//printf("%s << badSegments = %d\n", __FUNCTION__, badSegments);

	if (badSegments > (horizontalDivisions * verticalDivisions / 2)) {
		return false;
	}

	return true;

}

void drawMatchPaths(Mat& src, Mat& dst, vector<Point2f>& kp1, vector<Point2f>& kp2, const Scalar& color) {

	src.copyTo(dst);
	Point a, b;

	for (unsigned int iii = 0; iii < kp1.size(); iii++) {

		a = Point(kp1.at(iii).x * 16.0, kp1.at(iii).y * 16.0);
		b = Point(kp2.at(iii).x * 16.0, kp2.at(iii).y * 16.0);

		line(dst, a, b, color, 1, CV_AA, 4);

		// a = b;

	}
}

void drawMatchPaths(Mat& src, Mat& dst, vector<KeyPoint>& kp1, vector<KeyPoint>& kp2, vector<vector<DMatch> >& matches1to2) {

	//src.copyTo(dst);

	Point a, b;

	/*
	if (matches1to2.size() == 0) {
		printf("%s << No matches provided. Assuming all corresponding points match.\n", __FUNCTION__);

		for (int iii = 0; iii < kp1.size(); iii++) {
			a = Point(kp1.at(iii).pt.x * 8, kp1.at(iii).pt.y * 8);
			b = Point(kp2.at(iii).pt.x * 8, kp2.at(iii).pt.y * 8);

			line(dst, a, b, CV_RGB(0,0,255), 1, CV_AA, 3);
		}

		return;

	}
	*/

	for (unsigned int iii = 0; iii < matches1to2.size(); iii++) {
		for (unsigned int jjj = 0; jjj < matches1to2.at(iii).size(); jjj++) {

			//printf("%s << (%f,%f) vs (%f,%f)\n", __FUNCTION__, kp1.at(iii).pt.x, kp1.at(iii).pt.y, kp2.at(iii).pt.x, kp2.at(iii).pt.y);

			//printf("%s << (%d, %d) - (%d, %d) / %f\n", __FUNCTION__, iii, jjj, matches1to2.at(iii).at(jjj).trainIdx, matches1to2.at(iii).at(jjj).queryIdx, matches1to2.at(iii).at(jjj).distance);
			//cin.get();
			// matches1to2.at(iii).at(jjj).trainIdx

			a = Point(kp1.at(matches1to2.at(iii).at(jjj).queryIdx).pt.x * 16.0, kp1.at(matches1to2.at(iii).at(jjj).queryIdx).pt.y * 16.0);
			b = Point(kp2.at(matches1to2.at(iii).at(jjj).trainIdx).pt.x * 16.0, kp2.at(matches1to2.at(iii).at(jjj).trainIdx).pt.y * 16.0);

			line(dst, a, b, CV_RGB(0,0,255), 1, CV_AA, 4);

			circle(dst, b, 1, CV_RGB(255, 0, 0), 1, CV_AA, 4);

		}

	}
	//cin.get();
}

void markEdgyTracks(vector<Point2f>& pts, vector<uchar>& statusVec, cameraParameters& camData) {

	vector<Point2f> rpts;

	float minBorderDist = 15.0;


	redistortPoints(pts, rpts, camData.Kx, camData.distCoeffs, camData.expandedCamMat);

	Mat tmpDisp;

	tmpDisp = Mat::zeros(camData.expandedSize, CV_8UC3);

	Mat testMat;
	displayKeypoints(tmpDisp, rpts, tmpDisp, CV_RGB(0,255,0), 0);

	unsigned int edgyMarks = 0;

	float xDist, yDist, dist;

	for (int iii = pts.size()-1; iii >= 0; iii--) {

		xDist = min(((float) camData.expandedSize.width) - rpts.at(iii).x, rpts.at(iii).x);
		yDist = min(((float) camData.expandedSize.height) - rpts.at(iii).y, rpts.at(iii).y);
		dist = min(xDist, yDist);

		if (dist < minBorderDist) {

			if (statusVec.at(iii) != 0) {
				edgyMarks++;
			}
			statusVec.at(iii) = 0;
			rpts.erase(rpts.begin() + iii);
		}

	}

	displayKeypoints(tmpDisp, rpts, tmpDisp, CV_RGB(0,0,255), 0);

	//imshow("tmpDisp", tmpDisp);
	//waitKey(1);

	//printf("%s << edgyMarks = %d / %d\n", __FUNCTION__, edgyMarks, pts.size());
	//cout << "camData.Kx = " << camData.Kx << endl;

}

double estimateStability(Mat& img, Point2f& center, double radius) {
    double stability = 0.0;

    if (((center.x - radius) < 0) || ((center.y - radius) < 0) || ((center.x + radius) >= img.cols) || ((center.y + radius) >= img.rows)) {
		//printf("%s << Returning, error.\n", __FUNCTION__);
		return stability;
	}

    Mat patch;
	constructPatch(img, patch, center, radius, 15);

    double minVal, maxVal;
    minMaxLoc(patch, &minVal, &maxVal);

	double variance = getPatchVariance(patch);

    Mat largerMat;
    //patch /= maxVal;
    //resize(patch, largerMat, Size(patch.rows*4, patch.cols*4), 0, 0, INTER_NEAREST);

    // largerMat /=


	//imshow("patch", largerMat);
	//waitKey();

	//printf("%s << variance = %f\n", __FUNCTION__, variance);

	stability = 1 / variance;

	return stability;

}

double getPatchVariance(const Mat& patch) {
    double mean = 0.0;
	double sigma = 0.0;

	for (unsigned int iii = 0; iii < patch.rows; iii++) {
		for (unsigned int jjj = 0; jjj < patch.cols; jjj++) {
			mean += patch.at<double>(iii,jjj) / ((double) (patch.rows * patch.cols));
		}
	}

	for (unsigned int iii = 0; iii < patch.rows; iii++) {
		for (unsigned int jjj = 0; jjj < patch.cols; jjj++) {
			//printf("%s << (%f vs %f)\n", __FUNCTION__, patch.at<double>(iii,jjj), mean);
			sigma += pow((patch.at<double>(iii,jjj) - mean), 2.0) / ((double) (patch.rows * patch.cols));
		}
	}

	sigma = pow(sigma, 0.5);

	return sigma;
}
